
import math

def getDefaultRawZeroAngleDictionary():
	aDict = {}
	aDict[0] = 512
	aDict[1] = 792
	aDict[2] = 647
	aDict[3] = 362
	aDict[4] = 452
	aDict[5] = 512
	aDict[6] = 232
	aDict[7] = 377
	aDict[8] = 662
	aDict[9] = 572
	aDict[10] = 390
	aDict[11] = 242
	aDict[12] = 512
	aDict[13] = 634
	aDict[14] = 782
	aDict[15] = 512
	return aDict

def convertFromRadianToRaw(radian, servoId):
	raw = radian / 0.325 * 360 / (2*math.pi)
	raw += getDefaultRawZeroAngleDictionary()[servoId]
	if raw < 0:
		raw > 0
	if raw > 1024:
		raw = 1024
	return int(raw)

def convertFromRawToRadian(raw, servoId):
	radian = (raw - getDefaultRawZeroAngleDictionary()[servoId]) * 0.325 * 2 * math.pi / 360
	return radian

def convertRadianDictToRawDict(radianDict):
	rawDict = {}
	for k, v in radianDict.iteritems():
		rawDict[k] = convertFromRadianToRaw(v, servoId)

	return rawDict

